/*
*
==============================================================================
*  Name        : 
*  Part of     : Omen Game Engine
*  Description : 
*  Version     : 1.0
*
*  Copyright (c) 2013 Lauri Kortevaara.
* 
==============================================================================
*/

#define OMEN_SOURCE 1

#include <OmenMath.h>
#include <OmenVector3d.h>

#include <iostream>
#include <iomanip>
#include <sstream>

using namespace Omen;

///
/// <summary>Crete 4 by 4 identity matrix</summary>
///
Matrix4x4::Matrix4x4()
{
    memset(m,0,16*sizeof(Omen::Float));
    m[0] = m[5] = m[10] = m[15] = static_cast<Omen::Float>(1.0);  
}

///
/// <summary>
/// Constructs a 4 by 4 matrix using the following schema
///
/// [ m11 ][ m12 ][ m13 ][ m14 ]
/// [ m21 ][ m22 ][ m23 ][ m24 ]
/// [ m31 ][ m32 ][ m33 ][ m34 ]
/// [ m41 ][ m42 ][ m43 ][ m44 ]
/// </summary>
Matrix4x4::Matrix4x4(   const Omen::Float& _m11, const Omen::Float& _m12, const Omen::Float& _m13, const Omen::Float& _m14,
                        const Omen::Float& _m21, const Omen::Float& _m22, const Omen::Float& _m23, const Omen::Float& _m24,
                        const Omen::Float& _m31, const Omen::Float& _m32, const Omen::Float& _m33, const Omen::Float& _m34,
                        const Omen::Float& _m41, const Omen::Float& _m42, const Omen::Float& _m43, const Omen::Float& _m44 )
{
    m[0]= _m11; m[1]= _m12; m[2]= _m13; m[3]= _m14;
    m[4]= _m21; m[5]= _m22; m[6]= _m23; m[7]= _m24;
    m[8]= _m31; m[9]= _m32; m[10]=_m33; m[11]=_m34;
    m[12]=_m41; m[13]=_m42; m[14]=_m43; m[15]=_m44;
}

///
/// <summary></summary>
///
Matrix4x4::Matrix4x4( const Matrix4x4& other )
{
    m11=other.m[0];  m12=other.m[1];  m13=other.m[2];  m14=other.m[3];
    m21=other.m[4];  m22=other.m[5];  m23=other.m[6];  m24=other.m[7];
    m31=other.m[8];  m32=other.m[9];  m33=other.m[10]; m34=other.m[11];
    m41=other.m[12]; m42=other.m[13]; m43=other.m[14]; m44=other.m[15];
}

///
/// <summary></summary>
///
Matrix4x4::~Matrix4x4()
{
    memset(&m, 0, 16*sizeof(Omen::Float));
}

///
/// <summary>Returns the determinant of this 4x4 square matrix</summary
///  array index          |  Matrix index        | SubMatrix M11
///  [0]  [1]  [2]  [3]   |  [11] [12] [13] [14] | [22] [23] [24]
///  [4]  [5]  [6]  [7]   |  [21] [22] [23] [24] | [32] [33] [34]
///  [8]  [9] [10] [11]   |  [31] [32] [33] [34] | [42] [43] [44]
/// [12] [13] [14] [15]   |  [41] [42] [43] [44] | 
///
///
///
/// det = [11] * M11 - [12] * M12 + [13] * M13 -...

///
Omen::Float Matrix4x4::determinant() const
{
    Matrix3x3 subm[4];

    subm[0] = Matrix3x3(m[6],m[7],m[8],m[10],m[11],m[12],m[14],m[15],m[16]);
    subm[1] = Matrix3x3(m[5],m[7],m[8],m[9],m[11],m[12],m[13],m[15],m[16]);
    subm[2] = Matrix3x3(m[5],m[6],m[8],m[9],m[10],m[12],m[13],m[14],m[16]);
    subm[3] = Matrix3x3(m[5],m[6],m[7],m[9],m[10],m[11],m[13],m[14],m[15]);

    Omen::Float d1 = subm[0].determinant();
    Omen::Float d2 = subm[1].determinant();
    Omen::Float d3 = subm[2].determinant();
    Omen::Float d4 = subm[3].determinant();
    
    Omen::Float det = m[0]*d1 - m[1]*d2 + m[2] * d3 - m[3] * d4;
    
    return det;
}

///
// <summary></summary>
///
Matrix4x4&  Matrix4x4::inverted( bool* isInvertible )
{
    Omen::Float det = determinant();

    if( det == 0.0 )
    {
        if( isInvertible )
            *isInvertible = false;
        return *this;
    }
    else
        if( isInvertible )
            *isInvertible = true;
        

    det = 1/det;

    *this *= det;
    
    return *this;
}

///
/// <summary></summary>
///
bool Matrix4x4::isIdentity() const
{
    bool bIsIdentity = true;
    return bIsIdentity;
}

///
/// <summary></summary>
///
void Matrix4x4::lookAt( const Omen::Vector3d& eye, const Omen::Vector3d& target, const Omen::Vector3d& up )
{
/*
zaxis = normal(target - eye)
xaxis = normal(cross(up, zaxis))
yaxis = cross(zaxis, xaxis)

// Row major ordering
[        xaxis.x            ][            yaxis.x        ][           zaxis.x         ][ 0 ]
[        xaxis.y            ][            yaxis.y        ][           zaxis.y         ][ 0 ]
[        xaxis.z            ][            yaxis.z        ][           zaxis.z         ][ 0 ]
[-dot(xaxis, cameraPosition)][-dot(yaxis, cameraPosition)][-dot(zaxis, cameraPosition)][ 1 ]

// Column major ordering
[        xaxis.x            ][            yaxis.x        ][           zaxis.x         ][ 0 ]
[        xaxis.y            ][            yaxis.y        ][           zaxis.y         ][ 0 ]
[        xaxis.z            ][            yaxis.z        ][           zaxis.z         ][ 0 ]
[-dot(xaxis, cameraPosition)][-dot(yaxis, cameraPosition)][-dot(zaxis, cameraPosition)][ 1 ]
*/

    Vector3d zaxis = target;
    zaxis -= eye;
    zaxis.normalize();
    Vector3d xaxis = Vector3d(Vector3d::crossProduct(up,zaxis)).normalized();
    Vector3d yaxis = Vector3d::crossProduct(zaxis,xaxis);

    // Row major ordering
    m[0] = xaxis[0]; m[1] = yaxis[0];   m[2] = zaxis[0];    m[3] = static_cast<Omen::Float>(0.0);
    m[4] = xaxis[1]; m[5] = yaxis[1];   m[6] = zaxis[1];    m[7] = static_cast<Omen::Float>(0.0);
    m[8] = xaxis[2]; m[9] = yaxis[2];   m[10] = zaxis[2];   m[11] = static_cast<Omen::Float>(0.0);
    m[12] = -Vector3d::dotProduct(xaxis,eye); m[13] = -Vector3d::dotProduct(yaxis,eye); m[14] = -Vector3d::dotProduct(zaxis,eye);   m[15] = static_cast<Omen::Float>(1.0);

    // Column major ordering
    /*m[0] = xaxis.x; m[4] = yaxis.x;   m[8] = zaxis.x;    m[12] = -Vector3d::dotProduct(xaxis,eye);
    m[1] = xaxis.y; m[5] = yaxis.y;   m[9] = zaxis.y;    m[13] = -Vector3d::dotProduct(yaxis,eye);
    m[2] = xaxis.z; m[6] = yaxis.z;   m[10] = zaxis.z;   m[14] = -Vector3d::dotProduct(zaxis,eye);
    m[3 ] = static_cast<Omen::Float>(0.0); m[ 7] = static_cast<Omen::Float>(0.0); m[11] = static_cast<Omen::Float>(0.0);   m[15] = static_cast<Omen::Float>(1.0);*/

}

void Matrix4x4::setYawPitchRoll( Omen::Float yaw, Omen::Float pitch, Omen::Float roll )
{
    Omen::Float alpha = yaw;
    Omen::Float beta  = pitch;
    Omen::Float gamma = roll;

    // Before yaw pitch roll
    //std::cout << "Before yaw, pitch, roll:  " << yaw/Omen::PI_OVER_180 << ", " << pitch/Omen::PI_OVER_180 << ", " << roll/Omen::PI_OVER_180 << std::endl;
    //printMatrix(*this);

    m[0] = cos( gamma )*cos(alpha); m[4] = cos( gamma )*sin(alpha)*sin(beta)-sin(gamma)*cos(beta);  m[8] = cos(gamma)*sin(alpha)*cos(beta)+sin(gamma)*sin(beta); m[12] = 0.0f;
    m[1] = sin( gamma )*cos(alpha); m[5] = sin( gamma )*sin(alpha)*sin(beta)+cos(gamma)*cos(beta);  m[9] = sin(gamma)*sin(alpha)*cos(beta)-cos(gamma)*sin(beta); m[13] = 0.0f;
    m[2] = -sin(alpha);            m[6] = cos( gamma )*sin(beta);                                   m[10] = cos(alpha)*cos(beta);                               m[14] = 0.0f;
    m[3] = 0.0f;                    m[7] = 0.0f;                                                     m[11] = 0.0f;                                                 m[15] = 1.0f;

    //std::cout << "After yaw, pitch, roll:  " << yaw/Omen::PI_OVER_180 << ", " << pitch/Omen::PI_OVER_180 << ", " << roll/Omen::PI_OVER_180 << std::endl;
    //printMatrix(*this);
}

///
/// <summary>Return normal matrix of this matrix. E.g. A transpose of inverted 3 by 3 top-left submatrix</summary>
///
Matrix3x3    Matrix4x4::normalMatrix() const
{
    Matrix3x3 topLeft(  m[1], m[5],  m[9],  //  m[3]
                        m[2], m[6],  m[10], //  m[7]
                        m[3], m[7],  m[11]);// m[11] );
                    // m[ 4],m[ 8],  m[14],    m[15]

    // If top left submatrix is not invertible (e.g. it's determinant is 0, return 3x3 identity matrix)
    if( topLeft.determinant() == 0.0 )
        return Matrix3x3();

    topLeft = topLeft.inverted();
    topLeft = topLeft.transposed();
    
    return topLeft;    
}

///
/// <summary>Translates this Matrix by given 3d vector</summary>
///
void Matrix4x4::translate ( const Vector3d& vector )
{
    m[12]  += vector[0];
    m[13]  += vector[1];
    m[14]  += vector[2];
}

///
/// <summary>Translates this Matrix by given 2d coordinate</summary>
///
void Matrix4x4::translate( Omen::Float x, Omen::Float y )
{
    m[12] += x;
    m[13] += y;
}

///
/// <summary>Translates this Matrix by given 3d coordinate</summary>
///
void Matrix4x4::translate( Omen::Float x, Omen::Float y, Omen::Float z )
{
    m[12] += x;
    m[13] += y;
    m[14] += z;
}

///
/// <summary>Returns a transposed copy of this Matrix4x4</summary>
///
Matrix4x4     Matrix4x4::transposed() const
{
    Matrix4x4 copy;
    for( int i=0; i < 15; ++i )
    {
        int x = i / 4;
        int y = i % 4;
        copy.m[y*4+x] = m[x*4+y];
    }
    
    return copy;
}

///
/// <summary></summary>
///
bool     Matrix4x4::operator!=( const Matrix4x4& other ) const
{
    bool bFlag = false;
    for( unsigned char i = 0; i < 16; ++i )
        if(m[i] != other.m[i] )
        {
            bFlag = true;
            break;
        }
    return bFlag;
}
        
///
/// <summary></summary>
///
const Omen::Float&     Matrix4x4::operator() ( int row, int column ) const
{
    // check the given row and column
    if( row < 0 )
        row = 0;
    if( row > 3 )
        row = 3;
    if( column < 0 )
        column = 0;
    if( column > 3 )
        column = 3;

    return m[row*4+column];
}

///
/// <summary></summary>
///
Omen::Float& Matrix4x4::operator() ( int row, int column )
{
    // check the given row and column
    if( row < 0 )
        row = 0;
    if( row > 3 )
        row = 3;
    if( column < 0 )
        column = 0;
    if( column > 3 )
        column = 3;

    return m[row*4+column];
}
        
///
/// <summary>Multiples the components of other Matrix by corresponding components of this matrix</summary>
///
Matrix4x4& Matrix4x4::operator*= ( const Matrix4x4& o )
{
    Omen::Float _m11 = m11*o.m11 + m12*o.m21 + m13*o.m31 + m14*o.m41; 
    Omen::Float _m12 = m11*o.m12 + m12*o.m22 + m13*o.m32 + m14*o.m42; 
    Omen::Float _m13 = m11*o.m13 + m12*o.m23 + m13*o.m33 + m14*o.m43; 
    Omen::Float _m14 = m11*o.m14 + m12*o.m24 + m13*o.m34 + m14*o.m44;

    Omen::Float _m21 = m21*o.m11 + m22*o.m21 + m23*o.m31 + m24*o.m41; 
    Omen::Float _m22 = m21*o.m12 + m22*o.m22 + m23*o.m32 + m24*o.m42; 
    Omen::Float _m23 = m21*o.m13 + m22*o.m23 + m23*o.m33 + m24*o.m43; 
    Omen::Float _m24 = m21*o.m14 + m22*o.m24 + m23*o.m34 + m24*o.m44;

    Omen::Float _m31 = m31*o.m11 + m32*o.m21 + m33*o.m31 + m34*o.m41; 
    Omen::Float _m32 = m31*o.m12 + m32*o.m22 + m33*o.m32 + m34*o.m42; 
    Omen::Float _m33 = m31*o.m13 + m32*o.m23 + m33*o.m33 + m34*o.m43; 
    Omen::Float _m34 = m31*o.m14 + m32*o.m24 + m33*o.m34 + m34*o.m44;

    Omen::Float _m41 = m41*o.m11 + m42*o.m21 + m43*o.m31 + m44*o.m41; 
    Omen::Float _m42 = m41*o.m12 + m42*o.m22 + m43*o.m32 + m44*o.m42; 
    Omen::Float _m43 = m41*o.m13 + m42*o.m23 + m43*o.m33 + m44*o.m43; 
    Omen::Float _m44 = m41*o.m14 + m42*o.m24 + m43*o.m34 + m44*o.m44;
    
    m11 = _m11; m12 = _m12; m13 = _m13; m14 = _m14;
    m21 = _m21; m22 = _m22; m23 = _m23; m24 = _m24;
    m31 = _m31; m32 = _m32; m33 = _m33; m34 = _m34;
    m41 = _m41; m42 = _m42; m43 = _m43; m44 = _m44;
    
    return *this;
}

///
/// <summary>Multiples the components of this Matrix by given scalar</summary>
///
Matrix4x4& Matrix4x4::operator*= ( Omen::Float scalar )
{
    for( unsigned char i = 0; i < 16; ++i )
        this->m[i] *= scalar;

    return *this;
}

///
/// <summary>Adds the components of other Matrix to corresponding components of this matrix</summary>
///
Matrix4x4& Matrix4x4::operator+= ( const Matrix4x4& other )
{
    for( unsigned char i = 0; i < 16; ++i )
        this->m[i] -= other.m[i];

    return *this;
}

///
/// <summary>Subtracks components of this Matrix by corresponding components of other matrix</summary>
///
Matrix4x4& Matrix4x4::operator-= ( const Matrix4x4& other )
{
    for( unsigned char i = 0; i < 16; ++i )
        this->m[i] -= other.m[i];

    return *this;
}

///
/// <summary>Divides all components of this Matrix by given scalar</summary>
///
Matrix4x4& Matrix4x4::operator/= ( Omen::Float scalar )
{
    for( unsigned char i = 0; i < 16; ++i )
        this->m[i] /= scalar;

    return *this;
}

///
/// <summary>Returns true if given Matrix4x4 is the same Matrix as this</summary>
///
bool Matrix4x4::operator== ( const Matrix4x4& other ) const
{
    bool bEqual = true;
    for( unsigned char i = 0; i < 16; ++i )
    {
        if( other.m[i] != this->m[i] )
        {
            bEqual = false;
            break;
        }
    }
    return bEqual;
}

///
/// <summary>Returns true if given Matrix4x4 is the same Matrix as this</summary>
///
Matrix4x4& Matrix4x4::operator=( const Matrix4x4& other )
{
    memcpy(&m, other.m, 16*sizeof(Omen::Float) );
    return *this;
}

/// <summary> Returns the vector of given row </summary>
Vector3d Matrix4x4::row( unsigned int row ) const
{
    return Vector3d( m[row*4], m[row*4+1], m[row*4+2] );
}

/// <summary> Returns the vector of given column </summary>
Vector3d Matrix4x4::column( unsigned int column ) const
{
    return Vector3d( m[column], 
                     m[column+4], 
                     m[column+8] );
}

/// <summary> Returns the vector of given row </summary>
void Matrix4x4::setRow( unsigned int row, const Omen::Vector3d& v)
{
    m[row*4]   = v[0];
    m[row*4+1] = v[1];
    m[row*4+2] = v[2];
}

/// <summary> Returns the vector of given column </summary>
void Matrix4x4::setColumn( unsigned int column, const Omen::Vector3d& v ) 
{
    m[column] = v[0];
    m[column+4] = v[1];
    m[column+8] = v[2];
}

Matrix4x4::Matrix4x4Proxy Matrix4x4::operator[] ( int index ) const
{
    return Matrix4x4Proxy(this,index);
}

Matrix4x4::Matrix4x4Proxy& Matrix4x4::operator[] ( int index )
{
#pragma warning(disable:4172)
    return Matrix4x4Proxy(this,index);
#pragma warning(default:4172)
}

Vector3d  Matrix4x4::operator*( const Omen::Vector3d& v )
{
    return Omen::Vector3d( m[0]*v.x+m[4]*v.y+m[8]*v.z, m[1]*v.x+m[5]*v.y+m[9]*v.z, m[2]*v.x+m[6]*v.y+m[10]*v.z );
}

Omen::Matrix4x4 Matrix4x4::operator*( const Omen::Matrix4x4& m )
{
    Omen::Matrix4x4 m1 = m;
    m1 *= *this;
    return m1;
}

OMEN_API Omen::Vector3d operator*( const Omen::Matrix4x4& matrix, const Omen::Vector3d& v )
{
    return Omen::Vector3d( matrix[0][0]*v.x+matrix[0][1]*v.y+matrix[0][2]*v.z, 
                           matrix[1][0]*v.x+matrix[1][1]*v.y+matrix[1][2]*v.z, 
                           matrix[2][0]*v.x+matrix[2][1]*v.y+matrix[2][2]*v.z );
}

OMEN_API Omen::Matrix4x4 operator*( const Omen::Matrix4x4& m1, const Omen::Matrix4x4& m2 )
{
    Omen::Matrix4x4 m = m1;
    m *= m2;
    return m;
}

OMEN_API Omen::Matrix4x4 operator*( Omen::Matrix4x4& m1, Omen::Matrix4x4& m2 )
{
    Omen::Matrix4x4 m = m2;
    m *= m1;
    return m;
}


void Matrix4x4::rotateY( Omen::Float radAngle )
{
    m[0] = cos(radAngle);
    m[2] = -sin(radAngle);

    m[8] = sin(radAngle);
    m[10] = cos(radAngle);
}

void Matrix4x4::rotateX( Omen::Float radAngle )
{
    m[5] = cos(radAngle);
    m[6] = sin(radAngle);

    m[9]  = -sin(radAngle);
    m[10] = cos(radAngle);
}

void Matrix4x4::rotateZ( Omen::Float radAngle )
{
    m[0] = cos(radAngle);
    m[1] = sin(radAngle);

    m[4] = -sin(radAngle);
    m[5] = cos(radAngle);
}

Matrix4x4::operator float *()
{
    return static_cast<Omen::Float*>(m);
}


///
/// utility function for printing a fixed field right aligned float with 2 unit precision
///
static std::string printfloat( Omen::Float f )
{
    std::string str;
    std::stringstream ss(str);
    ss << std::setiosflags(std::ios::fixed)
          << std::setprecision(2)
          << std::setw(5)
          << std::right << f;
    return ss.str();
}

///
/// utility function for printing out the matrix contents in 
///
OMEN_API void printMatrix( const Omen::Matrix4x4& m, std::wstring& wstr )
{
    std::string str;
    std::stringstream sstr(str);
    sstr << "[" << printfloat(m[0][0]) << " " << printfloat(m[0][1]) << " " << printfloat(m[0][2])  << " " << printfloat(m[0][3]) << "]" << std::endl <<
            "[" << printfloat(m[1][0]) << " " << printfloat(m[1][1]) << " " << printfloat(m[1][2])  << " " << printfloat(m[1][3]) << "]" << std::endl <<
            "[" << printfloat(m[2][0]) << " " << printfloat(m[2][1]) << " " << printfloat(m[2][2])  << " " << printfloat(m[2][3]) << "]" << std::endl <<
            "[" << printfloat(m[3][0]) << " " << printfloat(m[3][1]) << " " << printfloat(m[3][2])  << " " << printfloat(m[3][3]) << "]" << std::endl << std::endl;
    str = sstr.str();
    wstr = std::wstring(str.begin(), str.end());
} 

/// EOF
